#include <stdlib.h>
#include "audio_mem.h"
#include "esp_log.h"

#include "istetho3_filter.h"

static const char *TAG = "ISTETHO3_FILTER";

#define FLT_ORDER         6
#define GAIN_B            10.0

#define  COEFTEST
#define SAMPLES8K
#define OLDPARAM

#ifdef COEFTEST

double BH[] = {
	// 40-150
	1.0, 0.0, 0.0, 0.0,
	0.0, 0.0, 0.0  
	};
double AH[]  = {
	// 40-150
	1.0, 0.0, 0.0, 0.0,
	0.0, 0.0, 0.0
	};

double BL[] = {
	// 120-450
	1.0, 0.0, 0.0, 0.0,
	0.0, 0.0, 0.0  
};
double AL[] = {
	// 120-450
	1.0, 0.0, 0.0, 0.0,
	0.0, 0.0, 0.0  
};
double BB[] = {
	// 120-450
	1.0, 0.0, 0.0, 0.0,
	0.0, 0.0, 0.0  
};
double AB[] = {
	// 120-450
	1.0, 0.0, 0.0, 0.0,
	0.0, 0.0, 0.0  
};

#else 	
// 8000Hz sample frequency
#ifdef  SAMPLES8K

#ifdef OLDPARAM
//心音滤波参数
// 40-150
double BH[] = {
	0.000074068986821906, 0.0, -0.000222206960465719, 0.0,
    0.000222206960465719, 0.0, -0.000074068986821906
};
double AH[]  = {
	// 40-150
	1.000000000000000, -5.816478759797879, 14.109042366221395, -18.269433765222434,
	13.318928739933627, -5.183329536833532, 0.841271002243478
};


//肺音滤波参数
// 120-450
double BL[] = {
    0.001706723119875, 0.0, -0.005120169359625, 0.0,
	0.005120169359625, 0.0, -0.001706723119875
};
double AL[] = {
	1.000000000000000, -5.391439783193984, 12.207190008693054, -14.860764788149357,
	10.260840957938246, -3.810413892483608, 0.594616240753234
};

//心肺音滤波参数
//滤波 40-450
double BB[] = {
	0.003100220581080,  0.0,	-0.009300661743239,		0.0, 
	0.009300661743239,	0.0,	-0.003100220581080 };

double AB[] = {
	1.000000000000000,  -5.328583786419774,  11.875342272561301, -14.176128171269955,   
	9.564299574427723,  -3.458602885390060,   0.523674010026533 };
#else
//心音滤波参数
//滤波 20-200
double BH[] = {
	0.000308121042223613, 0.0, -0.000924363126670840, 0.0,
    0.000924363126670840, 0.0, -0.000308121042223613
};
double AH[]  = {
    -5.710424330204874,  13.599014827618783, -17.287929180706229,  12.374047597409081, -4.728243771731989,   0.753534870712334
};

//肺音滤波参数
// 滤波 40-500
double BL[] = {
    0.004235990289153, 0.0, -0.012707970867460, 0.0,
	0.012707970867460, 0.0, -0.004235990289153
};
double AL[] = {
	1.000000000000000,  -5.248135591869182,  11.525298316168369, -13.567859204014521,   9.035980731945720,  -3.228816252982698,   0.483533345957529
};

//心肺音滤波参数
//滤波 20-500
double BB[] = {
	0.004750523611375,  0.0,	-0.014251570834124,		0.0, 
	0.014251570834124,	0.0,	-0.004750523611375 };

double AB[] = {
	1.000000000000000,  -5.233706398935526,  11.448991291159963, -13.411693962798001,   8.879572420260811,  -3.151475294630248,   0.468312111171713 };
#endif   // OLDPARAM
#endif

#ifdef  SAMPLES16K
//	16000KHz sample frequency

//心音滤波参数
// 滤波 20-200
double BH[] = {
		 0.000041185560705246 ,  0.00000000000000000, -0.123556682115738, 		 0.00000000000000000, 	 0.123556682115738,  0.00000000000000000,
		-0.000041185560705246,
};
double AH[]  = {
		  1.000000000000000, -5.856850016595896, 14.296040020348954,
     -18.615154068795334,  13.637729191017716,   -5.329906123230786,   
         0.868140997474275,
};


//肺音滤波参数
double BL[] = {
	// 滤波 40-500
		 0.000619945850000,  0.00000000000000000, -0.001859837550000,
		 0.00000000000000000,  0.001859837550000,  0.00000000000000000,
		-0.000619945850000 
};
double AL[] = {
	// 20-500
            1.000000000000000,  -5.630477844981993,  13.225371769979724, 
            -16.589487272549526,  11.721210194767890,  -4.423053462786468,  0.696436640236720};
//心肺音滤波参数

//滤波 40-450

double BB[] = {
           0.000699349649643 ,  0,   -0.002098048948930,  0,   0.002098048948930,
           0,  -0.000699349649643 };

double AB[] = { 
        1.000000000000000,  -5.619203369456989,  13.169010417098445, -16.477248412004904,  11.609815782707418,  -4.367910392566154,   0.685535977284661
};

#endif   

#endif

double BCoef[FLT_ORDER+1] = {0};
double ACoef[FLT_ORDER+1] = {0};
double Xin[FLT_ORDER+1] = {0};
double Yout[FLT_ORDER+1] = {0};


void *bpfilter_open( int type){
	int *ptype = audio_calloc(1, sizeof(int));
	int i = 0;
	for (i=0; i<=FLT_ORDER; i++) {
		Xin[i] = 0.0;
		Yout[i] = 0.0;
	}
	if(type == 0){
		// heart sound
		for ( i=0; i<=FLT_ORDER; i++ ) {
			ACoef[i] = AH[i];
			BCoef[i] = BH[i];
		}
	}else if(type == 1){
		// lung sound
		for ( i=0; i<=FLT_ORDER; i++ ) {
			ACoef[i] = AL[i];
			BCoef[i] = BL[i];
		}
	}else if(type == 2){
		// beats sound
		for ( i=0; i<=FLT_ORDER; i++ ) {
			ACoef[i] = AB[i];
			BCoef[i] = BB[i];
		}
	}
	ESP_LOGI(TAG, "filter_bandpass type %d",type);
	return ptype;
}

void bpfilter_close(void *handle){
	if(handle){
        audio_free(handle);
    }
}


// #define SHOWFITERINFO
int bpfilter_process(void *buffer, unsigned int bytes, int channels, void *handle, int type){
	//buffer is char, low byte first
	short *pData = buffer;
	int len = bytes/2;
	int i = 0, j = 0;
	double tempData;

#ifdef SHOWFITERINFO
	static int iCnt = 0;
	iCnt++;
	if ((iCnt%1000) == 0) {
		ESP_LOGI(TAG, "filterA = %d, len = %d, chnl = %d\n",type, bytes, channels);		
		ESP_LOGI(TAG, "Data = %d %d %d %d %d %d %d %d \n",pData[0], pData[1],pData[2],pData[3],pData[4],pData[5],pData[6],pData[7]);		
	}
#endif
/*	
	for(i=0;i<len;i++){
		for (j = FLT_ORDER; j>0; j--) {
			Xin[j] = Xin[j-1];
			Yout[j] = Yout[j-1];
		}
		Xin[0] =  pData[i]/32768.0 * GAIN_B;
		Yout[0] = BCoef[0] * Xin[0] + BCoef[1] * Xin[1] + BCoef[2] * Xin[2] + BCoef[3] * Xin[3] + BCoef[4] * Xin[4] + BCoef[5] * Xin[5] + BCoef[6] * Xin[6]
						- ACoef[1] * Yout[1] - ACoef[2] * Yout[2] - ACoef[3] * Yout[3] - ACoef[4] * Yout[4] - ACoef[5] * Yout[5] - ACoef[6] * Yout[6];						   

		//pData[i] = Xin[0] * GAIN_B + 0.5;
		tempData = Yout[0] * 32768.0 + 0.5;
		if (tempData> 32760)  tempData =  32760;
		if (tempData<-32761)  tempData = -32761;
		pData[i] =  tempData;

	}
*/	
	return bytes;
}
